
/*****************************************************************************\
 *      A small programm, which shows how to use collsision detection
\*****************************************************************************/

/*! @file
 *
 *  @brief A ball roles throw the other balls and print 'Hello World' on every
 *  	   collsision
 *
 *  @author Tobias Ehlgen
 *  -reimplemented by Weiyu Yi, wyi@tu-clausthal.de
 *
 *  @todo
 *    - the ground is only a box, wouldn't it better to implement a plane
 *      instead of the box
 *    - is the value verbose in col::init still used?
 *
 *  @flags
 *
 */


//---------------------------------------------------------------------------
//  Includes
//---------------------------------------------------------------------------

//include c++ basic function
#include <stdio.h>
#include <stdlib.h>

#if defined(__APPLE__)
#include <GLUT/glut.h>
#else
#include <GL/glut.h>
#endif

//include collision detection
#include <ColDopTree.h>
#include <Collision.h>
#include <ColUtils.h>

//include render library
#include <osg/ref_ptr>
#include <osg/Geode>
#include <osg/Geometry>
#include <osg/MatrixTransform>
#include <osg/StateSet>
#include <osg/Math>
#include <osg/StateAttribute>
#include <osg/PolygonMode>
#include <osg/Transform>
#include <osg/Drawable>
#include <osg/Shape>
#include <osg/ShapeDrawable>
#include <osg/PrimitiveSet>
#include <osgViewer/Viewer>
#include <osgGA/TrackballManipulator>

//include wrapper
#include <ColWrapperOpenSceneGraph.h>
#include <ColUtilsOpenSceneGraph.h>

using col::Quat;
using col::ColGeometry;
using col::Primitive;
using col::Point3;
using col::Vector3;
using col::Vector4;
using col::Matrix4;

using col::makeCube;
using col::makeSphere;
using col::makePlane;

using namespace osg;

/***************************************************************************\
 *                        Global variables                                 *
\***************************************************************************/

namespace
{

col::CollisionPipeline* colpipeline;
const int Space =2;         //Space among the balls

//variables which are used by the program to move the balls
unsigned int nobjects = 5;
int rightbound = nobjects * Space;  //max x-value
int leftbound = -2;                //min x-value
int direction = 0;
double angle = 10/3.1415926;

ref_ptr<Group> root;
ref_ptr<Group> ground;
osgViewer::Viewer viewer;

//set the level of detail for the collision detection
col::LevelOfDetectionE level_of_det = col::LEVEL_EXACT;
vector<const std::vector<col::Point3> *> fixed_points;
vector<const std::vector<col::Point3> *> moving_points;

// render options
Vec4f ground_color = Vec4f( 0.9, 0.9, 0.9, 0 );
//last ball is the moving ball
bool lastBallRotate = true;

// pseudo-random number generator (instead of drand48)
col::FibRand *      prng;

}

/***************************************************************************\
 * MovingObj
 *
 * this struct represents the balls
\***************************************************************************/

struct MovingObject
{
	MovingObject();

    //state
	col::ColID m_id;
	ref_ptr<Group> node;
};

MovingObject::MovingObject( void )
{
	node = new MatrixTransform();
	ref_ptr<MatrixTransform> mt = dynamic_cast<MatrixTransform*>( node.get() );
	Matrixf mat;
	mat.makeIdentity();
	mt->setMatrix( mat );
}

MovingObject* movingObject;


/******************************************************************************\
 * Callback class for collision detection
 *
 * If two objects, which register in the collision detection with this
 * callback class, are collide the operator ()  is called.
 * So everything you want to happen on collision can be write in this
 * function.
 * For more information about how to use the collision detection with more
 * than one Callback-Class see bowling.cpp.
\******************************************************************************/

class CollisionCallback : public col::Callback
{
public:
	CollisionCallback( MovingObject obj1, MovingObject obj2,
					   col::LevelOfDetectionE level );
    virtual ~CollisionCallback();
    virtual void operator () ( const col::Data *data ) throw ();
protected:
    MovingObject obj1;
    MovingObject obj2;
};


CollisionCallback::CollisionCallback( MovingObject inobj1, MovingObject inobj2,
									 col::LevelOfDetectionE inlevel )
:	col::Callback( inobj1.m_id, inobj2.m_id, false, true, inlevel ), 
	obj1(inobj1), obj2(inobj2)
{
}

CollisionCallback::~CollisionCallback()
{
}

//here you can implement the things which should happen on collision,
//in this example it isn't that much
void CollisionCallback::operator () (const col::Data* /*data*/) throw ()
{
    printf( "Collision detected: %d <---> %d \n", obj1.m_id, obj2.m_id );
    fflush(stdout);
}


/***************************************************************************\
 *                       Check and  process coll                           *
\***************************************************************************/

void move_and_check( void )
{
	Vec3f rotDir;
	double moveX;

    //move the last ball form one side to the other
    if (direction == 0)
    {
        moveX = -0.05;
        rotDir = Vec3f( 1.0, 1.0, 1.0 );
		rotDir.normalize();
    }
    else
    {
        moveX = 0.05;
        rotDir = Vec3f( -1.0, -1.0, -1.0 );
		rotDir.normalize();
    }
	
    //move the last ball
	ref_ptr<MatrixTransform> mt = dynamic_cast<MatrixTransform*>( movingObject[nobjects-1].node.get() );
	Matrixf mat = mt->getMatrix();
	mat( 3, 0 ) += moveX;

    if( mat( 3, 0 ) > rightbound)
        direction = 0;
    if( mat( 3, 0 ) < leftbound)
        direction = 1;

	//rotate the ball
	angle = 1.0/3.1415926;
	Matrixf rotationMatrix;
	rotationMatrix.makeIdentity(); 
	rotationMatrix.makeRotate( angle, rotDir );
	
    //should the rolling ball rotate
    if(lastBallRotate)
    {
		mat = rotationMatrix * mat;
    }

	//assign the matrix to transform
	mt->setMatrix( mat );

	//update pipeline
	for ( unsigned int i=0; i<nobjects; i++ )
	{
		void * tran = static_cast<void*>( &mat );
		colpipeline->moveObject( movingObject[i].m_id, tran, col::convertMatrixFromOpenSceneGraph );
	}
	
    //call collision detection
    colpipeline->check();
}


/***************************************************************************\
 * Create Scene
\***************************************************************************/

void createScene( void )
{
	double rad = 0.5;
	
	// create root
	root = new Group;

	// create ground
	ground = new MatrixTransform;

	// ground_geode
	ref_ptr<Geometry> ground_geom = new Geometry;
	col::makePlane( ground_geom.get(), 30, 30, 1, 1 );
	ref_ptr<Geode> ground_geode = new Geode;
	ground_geode->addDrawable( ground_geom.get() );

	// ground_matrixTransform
	Matrixf ground_matrix;
	ground_matrix.makeIdentity();
	ground_matrix.makeTranslate( 0, 0, -rad );
	ref_ptr<MatrixTransform> ground_mt = dynamic_cast<MatrixTransform*>( ground.get() );
	ground_mt->setMatrix( ground_matrix );
	ground_mt->addChild( ground_geode.get() );

	// color
	ref_ptr<Vec4Array> ground_colors = new Vec4Array();	
	ground_colors->push_back( ground_color );
	ground_geom->setColorArray( ground_colors.get() );
	ground_geom->setColorBinding( osg::Geometry::BIND_OVERALL );

	// add it to the root
	root->addChild( ground.get() );

    for (unsigned int i = 0; i < nobjects;i++)
    {
		// geometry and geode
		ref_ptr<Geometry> geom = new Geometry;
		col::makeSphere( geom.get(), 1, rad );
		ref_ptr<Geode> geode = new Geode;
		geode->addDrawable( geom.get() );

		// matrix
		Matrixf mat;
		mat.makeIdentity();
		mat.makeTranslate( i*Space, 0, 0 );

		// group and matrixTransform
		ref_ptr<MatrixTransform> mt = dynamic_cast<MatrixTransform*>( movingObject[i].node.get() );
		mt->setMatrix( mat );
		mt->addChild( geode.get() );

		// color
		ref_ptr<Vec4Array> colors = new Vec4Array();	
		Vec4f color = Vec4f( prng->frand(), prng->frand(), prng->frand(), 0 );
		colors->push_back( color );
		geom->setColorArray( colors.get() );
		geom->setColorBinding(osg::Geometry::BIND_OVERALL);
	
		// set state attribute
		ref_ptr<osg::StateSet> state = movingObject[i].node->getOrCreateStateSet();
		ref_ptr<osg::PolygonMode> pm = new osg::PolygonMode( osg::PolygonMode::FRONT_AND_BACK, osg::PolygonMode::LINE );
		state->setAttributeAndModes( pm.get(), osg::StateAttribute::ON | osg::StateAttribute::OVERRIDE );

		// add it to the root
		root->addChild( movingObject[i].node.get() );
    }
}

/***************************************************************************\
 *                              Main                                       *
\***************************************************************************/

int main(int argc, char **argv)
{
	// initialize global and static data
    prng = new col::FibRand( time(NULL) );
	col::DopTree::init();
	
    //init movingObjects
    movingObject = new MovingObject[nobjects];

	// initialize scene
	createScene();
	viewer.setSceneData( root.get() );
	viewer.setCameraManipulator( new osgGA::TrackballManipulator );

    //init collision detection
    colpipeline = new col::CollisionPipeline ("RollingBallCollisionThread"/*ThreadName*/,1 /*ThreadID*/);

    colpipeline->setUseHulls(false);
    colpipeline->verbose(false,false);

    //register objects
    for (unsigned int i=0; i<nobjects; i++)
	{
		ref_ptr<Geode> geode = dynamic_cast<Geode*>( movingObject[i].node->getChild( 0 ) );
		ref_ptr<Geometry> geom = dynamic_cast<Geometry*>( geode->getDrawable( 0 ) );
		movingObject[i].m_id = colpipeline->makeCollidable( static_cast<void *>( geom.get() ), col::convertGeometryFromOpenSceneGraph );
    }

    //register callbacks
    //here you can specify which callback class should be use.
    for ( unsigned int i=0; i<nobjects; i++ )
    {
        for ( unsigned int j=0;j<i; j++ )
        {
            colpipeline->addCallback( new CollisionCallback( movingObject[j], movingObject[i],
                                                             level_of_det) );
        }
    }

	while( ! viewer.done() )
	{
		move_and_check();

		viewer.frame();
	}

    return 0;
}

